Control system and method for multi-vehicle systems

ABSTRACT

The control system and method for multi-vehicle systems provides nonlinear model predictive control (NMPC) to regulate navigation of multiple autonomous vehicles (mobile robots) operating under automatic control. The system includes an NMPC controller and an NMPC algorithm. The NMPC controller includes an optimizer, a state predictor, and a state estimator. Data compression is accomplished using a neural networks approach.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to robotics, and particularly to a control system and method for multi-vehicle systems that uses nonlinear model predictive control to regulate navigation of multiple autonomous vehicles operating under automatic control.

2. Description of the Related Art

Researchers have addressed multi-vehicle control by implementing a potential fields formation control strategy, but they considered a point mass robot. What is needed, however, is to extend the fields formation control strategy to make one of the robots lead the others in an unknown environment, and at the same time, have all the agents in the fleet keep their formation shape, based on the potential fields.

Thus, a control system and method for multi-vehicle systems solving the aforementioned problems is desired.

SUMMARY OF THE INVENTION

The control system and method for multi-vehicle systems provides nonlinear model predictive control (NMPC) to regulate navigation of multiple autonomous vehicles (mobile robots) operating under automatic control. The system includes an NMPC controller and an NMPC algorithm. The NMPC controller includes an optimizer, a state predictor, and a state estimator. Data compression is accomplished using a neural networks approach.

These and other features of the present invention will become readily apparent upon further review of the following specification and drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram illustrating the complete architecture of a NMPC controller for a single vehicle in a control system and method for multi-vehicle systems according to the present invention.

FIG. 2 are plots illustrating estimated and predicted state given constraints and control inputs.

FIG. 3 is a schematic diagram illustrating sets and feasible trajectories.

FIG. 4 is a graph illustrating a warm start ellipsoid.

FIG. 5 is a graph illustrating optimized terminal region with and without warm starting.

FIG. 6 is a graph illustrating optimization with tightened constraints.

FIG. 7 is a schematic diagram showing relationship between tightened constraints, terminal set, and minimum step size.

FIG. 8 is a graph illustrating optimal cost with various disturbance levels.

FIG. 9 is a graph illustrating boundary points of one step controllability set calculation.

FIG. 10 is a graph illustrating one step controllability set calculation, target set, and tightened constraints.

FIG. 11 is a graph illustrating recursive one step controllable sets, boundary points and trajectories between boundaries.

FIG. 12 is a graph illustrating optimizal cost for bounary points of I step controllable sets.

FIG. 13 is a graph illustrating robust output feasible set with tightened constraints.

FIG. 14 is a graph illustrating state trajectory in phase with terminal constraints.

FIG. 15 is a graph illustrating time evolution of states.

FIG. 16 is a graph illustrating control history.

FIG. 17 is a graph illustrating evolution of optimized cost function.

FIG. 18 is a block diagram illustrating distributed control of multi agent systems.

FIG. 19 is a block diagram illustrating possible computing systems implementing multi-agent control systems.

FIG. 20 is a schematic diagram illustrating trajectory compression and recovery using a neural network.

FIG. 21 is a schematic diagram showing collision course avoidance.

FIG. 22 is a schematic diagram illustrating successful collision course avoidance.

FIG. 23 is a graph illustrating fleet trajectory of three AUV.

FIG. 24 are plots illustrating states of agents connected in a strongly connected network.

FIG. 25 are plots illustrating normalized cost of vehicles and interagent distances.

FIG. 26 is a schematic diagram illustrating network topology of weakly connected team.

FIG. 27 is a graph illustrating fleet trajectory of 5 AUV.

FIG. 28 are plots illustrating state of agents.

FIG. 29 is a plot illustrating small gain condition for a single agent.

Similar reference characters denote corresponding features consistently throughout the attached drawings.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

At the outset, it should be understood by one of ordinary skill in the art that embodiments of the present method can comprise software or firmware code executing on a computer, a microcontroller, a microprocessor, or a DSP processor; state machines implemented in application specific or programmable logic; or numerous other forms without departing from the spirit and scope of the method described herein. The present method can be provided as a computer program, which includes a non-transitory machine-readable medium having stored thereon instructions that can be used to program a computer (or other electronic devices) to perform a process according to the method. The machine-readable medium can include, but is not limited to, floppy diskettes, optical disks, CD-ROMs, and magneto-optical disks, ROMs, RAMs, EPROMs, EEPROMs, magnetic or optical cards, flash memory, or other type of media or machine-readable medium suitable for storing electronic instructions.

The control system and method for multi-vehicle systems provides nonlinear model predictive control (NMPC) to regulate navigation of multiple autonomous vehicles (mobile robots) operating under automatic control. The system 100 includes an NMPC controller and an NMPC algorithm. The NMPC controller includes an optimizer, a state predictor, and a state estimator. Data compression is accomplished using a neural networks approach. As shown in FIG. 1, the NMPC controller 102 includes an optimizer 104 in operable communication with a state predictor 106, which is in operable communication with a state estimator 108. Data compression is accomplished using a neural networks approach. The optimizer 104 has an output u_(t) ⁰ that feeds the multi-vehicles 110. A disturbance predictor 112 is also connected to the multi-vehicles 110 and shares with the multi-vehicles 110 a variable w_(t). The disturbance predictor 112 has an output {tilde over (w)}_(t), t+N_(p)−1 that feeds the state predictor 106. A single coordinate position y_(t) of the multi-vehicles 110 is combined with their velocity v_(t) as an error signal that feeds sensors 114: The sensors 114 have an output {tilde over (y)}_(t) that feeds the state estimator 108.

The multi-vehicles 110 have nonlinear discrete-time dynamics characterized by the relation:

x _(t+1) =f(x _(t) , u _(t) , w _(t)),   (1)

and the nonlinear output is:

y _(t) =h(x _(t))   (2)

Internal states x_(t), outputs y_(t), local control inputs u_(t) and external inputs w_(t) belong to the following constrained convex sets:

$\begin{matrix} \begin{bmatrix} {{x_{t} \in X \Subset R^{n}},} & {X,\left\{ {{x\text{:}\mspace{14mu} x_{\min}} \leq x \leq x_{\max} > 0} \right\}} \\ {{y_{t} \in Y \Subset R^{q}},} & {Y,\left\{ {{y\text{:}\mspace{14mu} y_{\min}} \leq y \leq y_{\max} > 0} \right\}} \\ {{u_{t} \in U \Subset R^{m}},} & {U,\left\{ {{u\text{:}\mspace{14mu} u_{\min}} \leq u \leq u_{\max} > 0} \right\}} \\ {{w_{t} \in W \Subset R^{p}},} & {W,\left\{ {{w\text{:}\mspace{14mu} w_{\min}} \leq w \leq w_{\max} > 0} \right\}} \end{bmatrix} & (3) \end{matrix}$

External input w will be later used to model the information communicated by other members of the team or obstacles. In the current context of a single robotic vehicle (agent), we can utilize it to model any disturbance affecting the agent (e.g. wing gust, water current, turbulence etc.) or information about obstacle it has to avoid. The disturbance evolves according to the following nonlinear mapping:

w _(t+1) =g(w _(t), Φ_(t)),   (4)

where φ is an unknown input vector, possibly random. Since w_(t) is not additive, it can also be used to represent plant uncertainty. The actual state of the system is x_(t), while the state predicted by a model at time t for future time instant t+l is {tilde over (x)}_(t,t+1), assuming that the model of the system is not perfect, such that the nominal model actually used for state prediction is:

{tilde over (x)} _(t+1) ={tilde over (f)}({tilde over (x)} _(t) , u _(t) , {tilde over (w)} _(t)).   (5)

Often, not all states are directly measurable, and when they are sensors may produce an output corrupted with noise and this lead to uncertainty. Therefore, the measured output is:

{tilde over (y)} _(t) =y _(t)+ξ_(y) _(t) , ξ_(y) ≦|ξ_(y) _(t) ≦ξ _(y)|.   (6)

Therefore, given the outputs measured by sensors, there is a need to estimate the states in a manner such that the effect of noise and uncertainty are mitigated. Assumption is that a mechanism of state estimation exists, such that the state is estimated with some bounded error ξ_(x), such that:

{tilde over (x)} _(t) ={tilde over (x)} _(t|t−1) +K _(t)({tilde over (y)} _(t) −h({tilde over (x)} _(t|t−1))).   (7)

where K_(t) is time varying nonlinear filter, which is assumed to be available and {tilde over (x)}_(t|t−1) is the prior estimate. In the present method, the assumption is that this filter exists, such that:

{tilde over (x)} _(t) =x _(t)+ξ_(x) _(t) , ξ_(x) ≦|ξ_(x) _(t) ≦ξ _(x)|.   (8)

Moreover, assume the existence of another estimator for w, which produces the estimate {tilde over (w)}, such that:

{tilde over (w)} _(t) =w _(t)+ξ_(w) _(t) , ξ_(w) ≦|ξ_(w) _(t) ≦ξ _(w)|.   (9)

Without exact knowledge of the evolution of w_(t,t+N) _(p) , it can only have an approximation {tilde over (w)}_(t,t+N) _(p) of it using a nominal model g(·) given by:

{tilde over (w)} _(t+1) ={tilde over (g)}({tilde over (w)} _(t)),   (10)

such that there is a bounded disturbance transition uncertainty due to disturbance model mismatch:

w _(t+1) =g(w _(t), Φ_(t))+e _(w) _(t) , e_(w) ≦|e_(w) _(t) |≦ē _(w).   (11)

Similarly, it is assumed that system model mismatch leads to system transition uncertainty

$\left. {{e_{x_{t}}\overset{\Delta}{=}{= {\overset{\sim}{f(}x_{t}}}},u_{t},w_{t}} \right) - {f\left( {x_{t},u_{t},w_{t}} \right)}$

such that:

{tilde over (f)}(x _(t) , u _(t) , w _(t))=f(x _(t) , u _(t) , w _(t))+e _(x) _(t) , e_(x) ≦|e_(x) _(t) |≦ē _(x).   (12)

Now, due to uncertainty, the constraint sets (3) for x and w will be larger than constraints sets for {tilde over (x)}, and {tilde over (w)}, such that:

$\begin{matrix} {{{\overset{\sim}{x}}_{t} \in {{\overset{\sim}{X}}_{t}\left( {{\overset{\_}{e}}_{x},{\overset{\_}{\xi}}_{x},{\overset{\_}{e}}_{w},{\overset{\_}{\xi}}_{w}} \right)} \Subset X},{{\overset{\sim}{y}}_{t} \in {{\overset{\sim}{Y}}_{t}\left( \overset{\_}{v} \right)} \Subset Y},{{\overset{\sim}{w}}_{t} \in {{\overset{\sim}{W}}_{t}\left( {{\overset{\_}{\xi}}_{w},{\overset{\_}{e}}_{w}} \right)} \Subset {W.}}} & (13) \end{matrix}$

Normally NMPC is used for state regulation, i.e., it will usually steer the state to the origin or to an equilibrium state x_(r)=r, where r is a constant reference. This is generally true for process industries. However, in mobile robotics, the control objective depends on the mission profile of the vehicle, as the target state may evolve over time, rather than being constant. Tracking and path tracking are two fundamental control problems in mobile robotics. For tracking problems, the objective is to converge to a time-varying reference trajectory x_(d)(t) designed separately. On the other hand, in path following applications, the objective is to follow a reference path parameterized by geometric parameters rather than time. The path following problem can be reduced to state regulation task. Therefore, the control strategy of MPC is explained using regulation problem as an example. Based on the control objective, let the vehicle have the finite-horizon optimization cost function given by:

$\begin{matrix} {{{J_{t}\left( {\overset{\sim}{x},u,\overset{\sim}{w},N_{c},N_{p},k_{f}} \right)} = {{\sum\limits_{l = t}^{t + N_{c} - 1}\; \left\lbrack {{h\left( {{\overset{\sim}{x}}_{l},u_{l}} \right)} + {q\left( {{\overset{\sim}{x}}_{l},{\overset{\sim}{w}}_{l}} \right)}} \right\rbrack} + {\sum\limits_{l = {t + N_{c}}}^{t + N_{p} - 1}\; \left\lbrack {\left( {{\overset{\sim}{x}}_{l},{k_{f}\left( {\overset{\sim}{x}}_{l} \right)}} \right) + {q\left( {{\overset{\sim}{x}}_{l},{\overset{\sim}{w}}_{l}} \right)}} \right\rbrack} + {h_{f}\left( {\overset{\sim}{x}}_{t + N_{p}} \right)}}},} & (14) \end{matrix}$

where N_(p) and N_(c) are prediction and control horizons. Cost function (14) consists of transition cost h, terminal cost h_(f) and robustness cost q (due to the effect of external input). Control sequence u_(t,t+Np) consists of two parts u_(t,t+N) _(c−1) and u_(t+N) _(c) _(,t+N) _(p−1) . The latter part is generated by terminal (also called auxiliary) control law u₁=k_(f), ({tilde over (x)}_(l)) for l=N_(c) while the former is finite horizon optimal control u_(t,t+Np) which is a finite horizon optimal control problem (FHOCP) solution.

The optimal control sequence that minimizes the finite horizon cost of eqn. (14) is:

$\begin{matrix} {{= {\underset{u \in U}{argmin}{J_{t}\left( {\overset{\sim}{x},{\overset{\sim}{w}}_{t,{t + N_{p}}},u_{t,{t + N_{p}}},N_{c},N_{p}} \right)}}},} & (15) \end{matrix}$

subject to (1) nominal state dynamics eqn. (5); (2) nominal disturbance dynamics eqn. (10); (3) Control constraint eqn. (3) and the tightened constraint sets relation (13); and (4) terminal state being constrained to an invariant terminal set X_(f) ∈ {tilde over (X)}_(t+N) _(c) , i.e.:

{tilde over (x)} _(t+l) ∈ X _(f) , ∀l=N _(c) , . . . , N _(p).   (16)

Suboptimal sequence u_(t,t+N) _(c−1) satisfying the constraints of eqns (1), (3) and (16) is called feasible control. In other words, a control input is feasible if and only if it provides a feasible solution to the finite horizon optimal control problem. Hence if a control input is admissible (u ∈ U), it is not necessarily feasible. For a given state the set of feasible inputs is a subset of the admissible inputs. The loop is closed by implementing only the first element of u_(t,t+N) _(c−1) ⁰ at each instant, such that the NLMPC implicit control law becomes:

Θ_(t)(

)=u _(t) ⁰({tilde over (x)} _(t) , {tilde over (w)} _(t) , N _(p) , N _(c)),   (17)

and the loop dynamic becomes:

x _(t+1) =f(x _(t), Θ_(t)(

), w _(t))=f _(c)(x _(t) , w _(t)).   (18)

with closed loop nonlinear map f_(c)(x, w). This process is repeated every sampling instant, as illustrated in plots 200 of FIG. 2. The overall control architecture 100 is shown in FIG. 1. To summarize, at time t, current state is sampled and an estimate of the disturbance is made, then cost eqn. (14) is minimized over a finite horizon N_(p), using N_(c) control adjustments and pre-computed terminal control law k_(f), such that system constraints (1)-(3) are satisfied in addition to state remaining in an invariant terminal set X_(f). Then the plant state is sampled again and the same optimization problem is solved again, yielding re-optimized control. Prediction horizon keeps being shifted forward and for this reason MPC is also called receding horizon control (RHC), though this is a slight abuse of notation (RH strategy along with model based optimization together forms the MPC strategy). The comprehensive strategy for robust nonlinear model predictive control is outlined in Table 1.

TABLE 1 Algorithm 1-Robust NMPC Control with Constraint Tightening  1: Input nominal model {tilde over (f)}({tilde over (x)},u,0), nominal constraints (3), receding horizon (RH) cost (14) and error bounds (7)-(12).  2:  procedure OFFLINE OPTIMIZATION  3:   Tighten constraints using Algorithm 2 for robustness  4:   Determine optimized terminal set X_(f) and terminal control k_(f) using Algorithm 3  5:   Warm-start Algorithm 5 with Algorithm 4.  6:   Determine One-step controllability set C₁(X_(f)) to ensure recursive feasibility using Algorithm 5.  7:   Determine Robust output feasibility set X_(MPC) using Algorithm 6.  8:  end procedure  9: Start system time at t, l = 0 11:  procedure (Online Optimization) 12:   Measure outputs {tilde over (y)}_(t+1) and disturbance {tilde over (w)}_(t+1) 13:   Estimate state {tilde over (x)}_(t+1) and disturbance {tilde over (w)}_(t+1) 14:   Solve finite horizon OCP at t + l for control u_(t+1,t+1,t+N) _(c) ⁰ 15:   Implement first element of optimized control u_(t) ⁰ 16:  end procedure System clock advances, l = l + 1 17: end while The two classes of optimization problems solved in Algorithm 1 are offline and online. This overall algorithm consists of various ingredient algorithms, described below.

As shown in FIG. 3, diagrams 300 illustrate the sets introduced by relation (3) and feasible trajectories. State constraint set X, tightened constraint set

, terminal set X_(f) and uncertainty ball β^(n)(c) are shown. With actual constraints X and W, the tightened constraints are given by:

$\begin{matrix} {{{\overset{\sim}{X}}_{t + l}\overset{\Delta}{=}{X \sim {\beta^{n}\left( {\overset{\_}{\rho}}_{x_{t + l}} \right)}}},{and}} & (19) \\ {{{\overset{\sim}{W}}_{t + l}\overset{\Delta}{=}{W \sim {\beta^{n}\left( {\overset{\_}{\rho}}_{w_{t + l}} \right)}}},} & (20) \end{matrix}$

for l=0, . . . , N_(P), where ρ _(x)and ρ _(w) are prediction error bounds defined using Lipschitz constants L** as:

$\begin{matrix} {{{\overset{\_}{\rho}}_{x_{t + l}}\overset{\Delta}{=}{{L_{fx}^{l}{\overset{\_}{\xi}}_{x}} + {{\overset{\_}{e}}_{x}\frac{L_{fx}^{l} - 1}{L_{fx} - 1}} + {{\overset{\_}{\xi}}_{w}L_{fw}\frac{L_{fx}^{l} - L_{gw}^{l}}{L_{fx} - L_{gw}}} + {{\overset{\_}{e}}_{w}\frac{L_{fw}}{L_{gw} - 1}\left( {\frac{L_{fx}^{l} - L_{gw}^{l}}{L_{fx} - L_{gw}} - \frac{L_{fx}^{l} - 1}{L_{fx} - 1}} \right)}}},} & (21) \\ {\mspace{79mu} {{{\overset{\_}{\rho}}_{w_{t + l}}\overset{\Delta}{=}{{L_{gw}^{l}{\overset{\_}{\xi}}_{w}} + {{\overset{\_}{e}}_{w}\frac{L_{gw}^{l} - 1}{L_{gw} - 1}}}},}} & (22) \end{matrix}$

Then, any (in general suboptimal) admissible control sequence u_(t,t+N) _(c) ⁻¹ and terminal control u_(t+N) _(c) _(,t+N) _(p−1) =k_(f)({tilde over (x)}_(t+N) _(c) _(,t+N) _(p) ⁻¹) that is feasible ({tilde over (x)}_(t+l) ∈ {tilde over (X)}_(t+1) u_(t,t+N) _(p−1) ∈ U,) and {tilde over (w)}_(t+l) ∈ {tilde over (W)}_(t+l) with respect to tightened constraints (21)-(22) applied to the actual system (1), guarantees the satisfaction of original constraints (3), i.e. x_(t+l) ∈ X and w_(t+l) ∈ W for l=0, . . . , N_(P) and x_(t) ∈ X_(MPC). The constraint tightening procedure is summarized in Table 2.

TABLE 2 Algorithm 2 Constraint Tightening 1: Given (i) nominal models {tilde over (f)}({tilde over (x)},u,{tilde over (w)})), {tilde over (g)} ({tilde over (w)}), (ii) uncertainty bounds ξ _(x), ξ _(w), ē_(x), ē_(x)w, and (iii) horizons N_(C), N_(P). 2:  procedure CONSTRAINT TIGHTENING 3:   Calculate Lipschitz constants of nonlinear maps {tilde over (f)}({tilde over (x)},u,{tilde over (w)}) and {tilde over (g)} ({tilde over (w)}). 4:   Calculate the prediction error bounds in (21) and (22). 5:   Tighten the constraints by Pontryagin difference as given in (19)-(20). 6:  end procedure

Constraint tightening (19)-(20) is novel as it is the first time that such a variety of uncertainty contributions have been considered simultaneously. Remarkably, the external input is not a constant or random unknown as is usually assumed, but herein it is considered to evolve according to an uncertain nonlinear map. Besides, estimation errors and prediction errors are also considered. This leads to very general bounds on prediction error, which can be specialized to specific cases (e.g. perfect measurement will mean ξx→0). Also worthy of note is the fact that we have not considered the model mismatch to be state-dependent, as it does not have obvious practical application in mobile robotics. In fact, if the system is very nonlinear, one cannot expect modeling error to reduce with state, as in many cases larger state amplitude offers better model fidelity. Moreover, the FHOCP is recursively feasible.

Satisfying constraints along the horizon depends on the future realization of the uncertainties, which are random. By assuming Lipschitz continuity of the nominal disturbance and state models it is possible to compute bounds on effect of the evolving uncertainties on the system. Since, our system consists of many possible sources of uncertainty, the bound calculated will be much more involved and comprehensive than those presented in existing literature.

With respect to the convex optimal control problem (OCP (A)) for maximizing a terminal constraint set, the volume of terminal constraint set X_(f)(a)={{tilde over (x)}: {tilde over (x)}^(T) Q_(f) {tilde over (x)}≦a}, for a>0, within set M defined in (4.13) with cost functional (3.58), is maximized for matrix variables

$W_{1}\overset{\Delta}{=}{{Q_{f}^{- 1} \in {\mspace{14mu} {and}\mspace{14mu} W_{2}}}\overset{\Delta}{=}{{KQ}_{f}^{- 1} \in}}$

by solving:

$\begin{matrix} {{\min\limits_{W_{1},W_{2},a}{\log \; {\det \left( {a\; W_{1}} \right)}^{- 1}}},} & (23) \\ {{W_{1} = {W_{1}^{T} > 0}},} & (24) \\ {{a > 0},} & (25) \\ {{\begin{bmatrix} W_{1} & \left( {{A_{v}W_{1}} + {B_{v}W_{2}}} \right)^{T} & {W_{1}\left( {Q - \overset{\sim}{S}} \right)}^{1/2} & {W_{2}^{T}R^{1/2}} \\ * & W_{1} & 0 & 0 \\ * & * & I & 0 \\ * & * & * & I \end{bmatrix} \geq 0},{{{for}\mspace{14mu} v} = 1},\ldots \mspace{14mu},{\overset{\_}{v}.}} & (26) \\ {\begin{bmatrix} {1\text{/}a} & \left( {{{\overset{\_}{c}}_{v}W_{1}} + {{\overset{\_}{d}}_{v}W_{2}}} \right)^{T} \\ * & W_{1} \end{bmatrix} \geq 0.} & (27) \end{matrix}$

Matrix {tilde over (S)} is a positive definite matrix. Additionally, if it is required to converge with a given rate â then the OCP is subject to another condition:

$\begin{matrix} {\begin{bmatrix} {- \left( {Q - \left( {\overset{\sim}{S} + {\hat{\alpha}I_{n}}} \right)} \right)} & W_{2}^{T} \\ * & R^{- 1} \end{bmatrix} \geq 0.} & (28) \end{matrix}$

Plots 500 of FIG. 5 indicate comparative optimized terminal regions with and without the use of the warm start procedure of Algorithm 4 by Algorithm 3. The optimized terminal set and terminal control determination procedure is summarized in Table 3.

TABLE 3 Algorithm 3 Optimizing Terminal Region and Control  1: Given nominal models {tilde over (f)}{tilde over ( )}({tilde over (x)},u,0)), and cost weights Q, R and S.  2: Select {tilde over (S)} ε

 ^(n×n), such that −q({tilde over (x)},{tilde over (w)}) + ψ({tilde over (w)}) ≦ {tilde over (x)}_(c) ^(i){tilde over (S)}{tilde over (x)}  3: Get initial guess values of Q_(f) as Q_(f) ^(∞) and K as K^(∞) by Algorithm 4  4:  procedure CONVEX OPTIMIZATION  5:   Solve convex OCP (A) using parameterized state and control constraints (3) subject to (24)-(27).  6:   if X_(f) ⊂ {tilde over (X)}_(t+N) _(p) then  7:    Go to 11  8:   else  9:    Solve convex OCP (A) subject to (24)-(28). 10:   end if 11:  end procedure End algorithm; accept optimal values of Q_(f),  K and a.

Most modern software packages select the initial iterate internally. For example, the SDPT3 semidefinite programming package algorithms can start with an infeasible starting point, as the algorithms try to achieve feasibility and optimality of its iterates simultaneously.

However, the performance of these algorithms is quite sensitive to the choice of the initial iterate. Reasonable initial guesses are often crucial, especially in non-convex optimization. It is advisable to choose an initial iterate that at least has the same order of magnitude as the optimal solution. Therefore the present invention provides an initial guess of optimization variables to warm-start Algorithm 3 by solving discrete-time algebraic Riccati equations (DARE) at each vertex point as follows:

Q _(f) _(v) ^(∞)=(Q−{tilde over (S)})+A _(v) ^(T) (Q _(f) _(v) ^(∞) +Q _(f) _(v) ^(∞) B _(v)(R+B _(v) ^(T) Q _(f) _(v) ^(∞) B _(v))⁻¹ B _(v) ^(T) Q _(f) _(v) ^(∞))A _(v),   (29)

where Q_(f) _(v) ^(∞)is the solution to the DARE above. The control gain computed from eqn. (29) is given as:

K _(v) ^(∞)=(R+B _(v) ^(T) Q _(f) _(v) ^(∞) B _(v))⁻¹ B _(v) ^(T) Q _(f) _(v) ^(∞) A _(v).   (30)

It should be understood that v takes on values of K_(v) ^(∞) and Q_(f) _(v) ^(∞). Therefore, we will solve another optimization problem that finds the maximum volume ellipsoid which is confined in the intersection of all the {tilde over (v)} vertex ellipsoids. This will serve as the initial guess for W₁ ⁰=Q_(f) _(v) ^(∞). It is important to note that the initial guess is based on solution of unconstrained algebraic Riccati equations (29). Therefore, we formulate the following convex OCP by exploiting the S-procedure. Thus, warm-start for Algorithm 3 assumes v vertex ellipsoids

${ɛ_{v}^{n}\left( Q_{fv}^{\infty} \right)}\overset{\Delta}{=}\left\{ {\overset{\sim}{x} \in {{{\mathbb{R}}^{n}\text{:}{\overset{\sim}{x}}^{T}Q_{fv}^{\infty}\overset{\sim}{x}} \leq 1}} \right\}$

which are solutions of Riccati equations (29), the maximum volume ellipsoid in the intersection of all ε_(v) ^(n)is obtained by solving the following convex OCP (2) for some Lagrangian variables t_(v):

$\begin{matrix} {{\min\limits_{{\overset{\sim}{W}}_{\infty}}{- {\log \; \det {\overset{\sim}{W}}^{\infty}}}},{{subject}\mspace{14mu} {to}}} & (31) \\ {{\overset{\sim}{W}}^{\infty} > 0} & (32) \\ {1 \geq t_{v} \geq 0} & (33) \\ {{{{t_{v}{\overset{\sim}{W}}_{v}^{\infty}} - {\overset{\sim}{W}}^{\infty}} \geq {0\mspace{14mu} {for}\mspace{14mu} v}},\ldots \mspace{14mu},{{{\overset{\_}{v}.\; {Here}}\mspace{14mu} {\overset{\sim}{W}}^{\infty}}\overset{\Delta}{=}\mspace{14mu} {{\left( Q_{f}^{\infty} \right)^{- 1}\mspace{14mu} {and}\mspace{14mu} {\overset{\sim}{W}}_{v}^{\infty}}\overset{\Delta}{=}{\left( Q_{fv}^{\infty} \right)^{- 1}.}}}} & (34) \end{matrix}$

FIG. 4 illustrates the warm start ellipsoids 400 used in Algorithm 4 as applied to an exemplary case. Plot 600 of FIG. 6 illustrates terminal region X_(f) optimized with Algorithms 3 and 4 for an exemplary case. Tightened constraints from Algorithm 2 are also shown. The warm-start procedure is summarized in Table 4.

TABLE 4 Algorithm 4 Warm-Start Procedure for Algorithm 3 1: procedure CONVEX OPTIMIZATION 2:  Solve Riccati equations (29) for vertex values of Q_(fv) ^(∞) 3:  Solve convex OCP (2) to obtain Q_(f) ^(∞) 4:  Calculate K^(∞) by solving (30) for Q_(f) ^(∞) at A₀ andB₀, i.e. linearization of origin 5: end procedure End algorithm and pass Q_(f) ^(∞) and K^(∞) to Algorithm 3

Regarding Determination of 1-Step Controllable Set to Terminal Constraint Set, the maximum allowable uncertainty is bounded by the minimum size of the 1-step controllable set to X_(f), denoted by C₁(X_(f)). In particular, this bound on uncertainty was shown to ensure recursive feasibility. Therefore, it is imperative to determine the minimum size of C₁(X_(f)), i.e., the minimum size of 1-step controllability set to terminal set X_(f)is defined as:

$\begin{matrix} {\overset{\_}{d}\overset{\Delta}{=}{{{dist}\left( {{{\overset{\sim}{X}}_{{t + N_{c}},}\backslash {C_{1}\left( {X_{f},{\overset{\sim}{X}}_{{t + N_{c}},}} \right)}},X_{f}} \right)}.}} & (35) \end{matrix}$

The relationship 700 between {tilde over (X)}_(t+N) _(c) _(,) C₁ (X_(f))and d is illustrated in FIG. 7. It is clear that to find d, we must know the topology of C₁(X_(f)). There are various techniques for estimating one-step controllability to given sets. The present method contemplates an explicit method based on min-max optimization to give better estimates of C₁(X_(f)). Let the boundary of X_(f)and C₁(X_(f)) be denoted by ∂(X_(f)) and {tilde over (x)}_(C) ₁ ^(i) ∈ ∂ (C₁(X_(f))), respectively. The One Step Controllable Set to X_(f) Determination procedure summarized in Table 5 is presented for this purpose.

TABLE 5 Algorithm 5 Determining One Step Controllable Set to X_(f) 1: Divide the boundary of terminal set ∂(X_(f)) into Ñ steps. 2: Solve OCP (3) to find points {tilde over (x)}_(c) ^(i) ε ∂(C₁(X_(f)))for i = 1, . . . , N. 3: Calculate minimum size of C₁(X_(f)) as d = min(|{tilde over (x)}_(c) ₁ ¹ − {tilde over (x)}_(f) ¹|, . . . , |{tilde over (x)}_(C) ₁ ^(N) − {tilde over (x)}_(f) ^(N) |) for {tilde over (x)}_(f) ^(i) ε ∂(X_(f)) and i = 1, . . . , N

OCP (3), Min-Max Optimization of One-Step Robust Controllability Set is as follows. Given the target set X_(f), tightened constraints defined in (19)-(20) and nominal constraints (3), let the boundary of X_(f) be discretized appropriately into N point x _(f) ^(i) ∈ ∂(X_(f)) for i=1, . . . , N. Then, the one-step robust controllability set C₁(X_(f)) is obtained by solving the following N min-max OCPs:

$\begin{matrix} {{{\overset{\sim}{x}}_{c_{i}}^{i} = {\max\limits_{\overset{\sim}{w}}\left( {\min\limits_{u}\left( {- {\log \left( {{\overset{\sim}{x}}_{C_{1}}^{i}Q_{f}^{\infty}} \right)}} \right)} \right)}},} & (36) \end{matrix}$

for i=1, . . . , N, subject to:

{tilde over (x)} _(f) ^(i) ={tilde over (f)}({tilde over (x)} _(c) ₁ ^(i) , u, {tilde over (w)})   (37)

1−{tilde over (x)} _(c1) ^(i) Q _(f) ^(∞) {tilde over (x)} _(c) ₁ ^(i)≦0   (38)

{tilde over (x)}_(c1) ^(i) ∈ {tilde over (X)}_(N) _(c) ⁻¹, u ∈ U, {tilde over (w)} ∈ {tilde over (W)}_(N) _(c) ⁻¹,   (39)

The boundary of C₁(X_(f)) is given as ∂ (C₁(X_(f)))={{tilde over (x)}_(c1) ¹, ∀i=1, . . . , N}. Notice that even though cost functional (36) is convex, the overall OCP is not convex due to presence of nonlinear constraints (37) and (38). Due to non-convexity, it is important to have a good initial guess. This can be easily accomplished by choosing initial guess in the sector of state space where the half space containing x_(f) ^(i) lies. Cost functional (36) is the convex form of ({tilde over (x)}_(c1)Q_(f){tilde over (x)}_(c) ₁ ^(i))⁻¹, minimizing which maximizes the distance from X_(f)={{tilde over (x)}:{tilde over (x)}Q_(f){tilde over (x)}≦1}. Constraint (37) ensures that {tilde over (x)}_(c1) ^(i) is the point from which the point {tilde over (x)}_(f) ^(i) ∈ ∂(X_(f)) can be reached in exactly one step. Constraint (38) ensures that is outside X_(f). Finally the cost is minimized with control u, but maximized with respect to the disturbance {tilde over (w)} to account for the worst case of disturbance input.

Plot 800 of FIG. 8 shows an optimal cost graph of a nonlinear oscillator with various disturbance levels using Algorithm 5. Algorithm 5 may be applied to determine the one-step robust controllability set to terminal set X_(f). Plot 900 of FIG. 9 shows the boundary points of one step controllability set calculated using algorithm 5 in an exemplary case. Plot 1000 of FIG. 10 shows one-step controllability set calculated using algorithm 5 for the exemplary case.

Algorithm 6 extends algorithm 5 to solve for the entire feasibility region of for the MPC algorithm. Robust one-step controllability set C₁(X_(f)) contains the target set X_(f), i.e. X_(f) ⊂ C(X_(f)). Robust one-step controllability set C(X_(f)) to the terminal set X_(f), is contained in the one-step controllability set of robust output feasible set X_(MPC), i.e.:

C ₁(X _(f))⊂ C ₁( X _(mpc))   (40)

Robust one-step controllability set C(X_(f)) to the terminal set X_(f) can be written as a finite union of polyhedra. The one-step controllable set operator can be used recursively to define l-step controllable set C₁(X_(f)) as follows (for l≧2):

C _(l)(X _(f))=C ₁(C_(l−1)(X _(f))).   (41)

The boundary of target set, i.e. ∂(X_(f)) is included in the one step controllable set C₁ (X_(f)):

∂(X_(f)) ⊂ C₁(x_(f)).   (42)

Given the terminal set X_(f), tightened constraints {tilde over (x)} ∈ {tilde over (X)}_(t+1), {tilde over (w)} ∈ {tilde over (W)}_(t−1), for l=1, . . . , N_(c) and control constraint u ∈ U, the robust feasibility set is obtained by N_(c)applications of the one-step controllable set operator C_(∞)(·) by recursively solving OCP (3), such that:

$\begin{matrix} {X_{MPC} = {{\overset{l = N_{c}}{\bigcup\limits_{l = 2}}{C_{1}\left( {C_{l - 1}\left( X_{f} \right)} \right)}}\bigcup{C_{1}\left( X_{f} \right)}\bigcup X_{f}}} & (43) \end{matrix}$

which can be generalized as follows:

$\begin{matrix} {X_{MPC} = {{C_{l}\left( {C_{N_{c} - 1}\left( X_{f} \right)} \right)} = {{{C_{1}\left( {C_{1}\left( {C_{N_{c} - 2}\left( X_{f} \right)} \right)} \right)}\mspace{14mu} \ldots} = {{C_{1}\left( {C_{1}\left( \; {\ldots \mspace{14mu} {C_{1}\left( X_{f} \right)}} \right)} \right)}.}}}} & (44) \end{matrix}$

Thus, the recursive X_(MPC) Feasibility Region determination procedure is summarized in Table 6.

TABLE 6 Algorithm 6 Determining Feasibility Region X_(MPC)  1: Determine C₁(X_(f)) by using Algorithm 5, given as {tilde over (x)}_(C) ₁ ^(i) ε ∂(C₁(X_(f)) for i = 1, . . . , N.  2:  procedure RECURSIVE ESTIMATION OF X_(MPC)  3:   for l = 2, . . . , N_(C) do  4:    if l = 2 then  5:     Solve OCP (3) with target set C₁(X_(f)) to obtain C₂(X_(f)) = C₁(C₁(X_(f)))  6:    else  7:     Solve OCP (3) with target set C_(l-1)(X_(f)) to obtain C_(l)(X_(f)) = C₁(C_(l-1)(X_(f)))  8:    end if  9:   end for 10:  Determine X_(MPC) according to (43). 11:  end procedure

The method given in Algorithms 5-6 is computationally demanding. However, all algorithms in this chapter are for used offline in the proposed MPC scheme, therefore computational burden is not an overriding concern. However, we must provide the caveat that choosing initial conditions for higher dimension systems is far less intuitive. In that case Algorithms 5-6 should be implemented in a heuristic (non-gradient-based approaches) to avoid the problems of local minima.

Plot 1100 of FIG. 11 illustrates an example of recursive one-step controllable sets. Circles indicate set boundary points and dotted lines show one step trajectories between boundaries of successive sets. Plot 1200 of FIG. 12 illustrates optimal cost for boundary points of 1-step controllable sets using algorithm 6 for the exemplary nonlinear oscillator case. Plot 1300 of FIG. 13 illustrates Robust Output Feasible Set X_(MPC) with Tightened Constraints. Boundaries of X_(MPC) coincide with tightened constraint {tilde over (X)}_(t+N) _(c) , the online part of Algorithm 1, implemented using fmincon package of Matlab. The goal is to regulate the state to the origin, without violating any constraints. It can be observed that the state does not converge to zero, since only practical stability is guaranteed. Plot 1400 of FIG. 14 shows the state trajectory in the phase plane, along with terminal region X_(f) and tightened constraints. Evolution of the states with time is shown in plot 1500 of FIG. 15, where it is again clear that the state does not converge to the origin due to the presence of uncertainty. The control input calculated by application of Algorithm 1 is shown in plot 1600 of FIG. 16. The figure shows how the maximum control authority is utilized initially without violation of this constraint, which is a unique feature of NMPC. Plot 1700 of FIG. 17 shows the evolution of the exemplary cost functional (3.58), which decreases monotonously, as expected. Computation time for offline part of Algorithm 1 was 320 seconds and for 50 seconds of simulation, the online algorithm took 39 seconds of computation time, on an Intel Core i5-4210U 2.7 GHz machine with 4 GB memory. This is adequate for online implementation, and can be further improved with dedicated code and hardware.

This method contemplates the distributed control of a fleet of autonomous agents. Often the main task in multi-vehicle cooperative control is formation. Formations control means the ability to move the entire fleet with a common speed and heading. This invariably means that the vehicles in the team should be able to either sense the states of team members, or receive state information from other team members. In most cases however, the communication occurs wirelessly as the agents are spread over a large area or it is not possible to maintain tethered connection network due to movement of vehicles and presence of obstacles. Also due to mobile nature of these vehicles, the on-board computational power is limited due to size and power budgets. Therefore, distributed control, 1800 as shown in FIG. 18, is often the only practical control architecture. Computer block 1900 (shown in FIG. 19) is an exemplary means for performing distributed control 1800 and may have a computing system 1910 comprised of I/O devices 1960 connected to I/O interfaces 1950, and which are, in turn, connected to processor 1920, which is connected to network interfaces 1970, program data 1936, applications 1934, operating system 1932 and system memory 1930. The processor may be connected to storage devices 1940. Additional interface may be used to connect to other computer systems 1980.

There are three basic elements in multi-agent formation control; Cohesion: attraction to distant neighbors up to a reachable distance. Alignment: velocity and average heading agreement with neighbors. Separation: repulsion from neighbors within some minimal distance. This is also called collision avoidance. Formation control without collision avoidance is also called state synchronization.

In a dynamic neural network based adaptive control scheme for distributed fleet state synchronization, without the need to know local or leader (nonlinear) dynamics. Lyapunov analysis is used to derive tuning rules, with the implicit need for persistent excitation, for both strongly connected and weakly (simply) connected networks. Plot 2300 of FIG. 23 illustrates three agents in a strongly connected network. Plots 2400 of FIG. 24 illustrates the states of the three agents in the strongly connected network. However, delays, asynchronous measurements, collision avoidance and limits on control actuation forces are not considered. In a similar approach, synchronization of nonlinear Lagrangian systems with linear-in-parameter model uncertainties has been solved using distributed adaptive back-stepping and adaptive redesign all agents are assumed to have access to group reference trajectory, which constitutes a further limitation. Synchronization of a fleet of nonlinear Euler-Lagrangian systems has also been achieved using distributed H_(∞) controllers robust to model uncertainties and disturbances in fixed and switching network topologies guaranteeing input to state stability (ISS).

We address the problem of leader-follower formation control of constrained autonomous vehicles subject to propagation delays. We consider the simultaneous presence of six sources of uncertainty: (i) error in estimating current state; (ii) error in estimating current external input (disturbance or external information); (iii) error in predicting future system state due to model mismatch; (iv) error in predicting future external input due to disturbance model mismatch (disturbance model is another uncertain dynamic system with unknown in-put); (v) error in approximating trajectory due to data compression; and (vi) error in approximating the last segments (tail) of the compressed trajectory due to propagation delays.

Limited network throughput demands reduction in packet size. The proposed approach achieves formation tracking through NMPC such that each agent performs local optimization based on planned approximate trajectories received from its neighbors. Since exchanging the entire horizon will increase packet size proportional to length of prediction horizon, the trajectory is compressed using neural networks, which is shown to reduce the packet size considerably. Moreover, the method allows the agents to be heterogeneous, make asynchronous measurements and have different local optimization parameters. Correction for propagation delays is achieved by time-stamping each communication packet. Collision avoidance is achieved by formulating a novel spatial-filtered potential field, which is activated in a “zone of safety” around the agent's trajectory. New theoretical results are presented along with validating simulations.

OCP (4): Consider a set of N agents, where each agent is denoted as A^(i) with i=1, . . . , N. Each agent has the following open loop nonlinear discrete-time dynamics described by

x _(t+1) ^(i) =f ₀ ^(i)(x _(t) ^(i) , u _(t) ^(i)), ∀i≧0,i=1, . . . , N   (45)

where f₀ ^(i) is a nonlinear map for local open loop dynamics, x_(t) ^(i), u_(t) ^(i) are states and controls local to agent A_(i). These variables belong to the following constraint sets:

$\begin{matrix} {{{x_{t}^{i} \in X^{i} \Subset R^{n^{i}}},{X^{i}\overset{\bigtriangleup}{=}\left\{ {x^{i}:{x_{\min} \leq x^{i} \leq x_{\max}^{i} > 0}} \right\}}}{{u_{t} \in U^{i} \Subset R^{m^{i}}},{U^{i}\overset{\bigtriangleup}{=}{\left\{ {u^{i}:{u_{\min} \leq u^{i} \leq u_{\max}^{i} > 0}} \right\}.}}}} & (46) \end{matrix}$

One can observe that the agents' dynamics (1) are decoupled from each other in open loop. This is the standard case for most formation control problems. Focusing on team of dynamically decoupled agents and due to measurements corrupted with sensor noise, we assume that local states are estimated (locally) with bounded error ξxi, such that:

{tilde over (x)} _(t) ^(i) =x _(t) ^(i)+ξ_(x) _(t) ^(i), ξ _(x) ^(i)≦|ξ_(x) _(t) ^(i)≦ξ _(x) ^(i)|.   (47)

Even though the agents are dynamically decoupled, they need to cooperate with each other to perform the formation keeping task. To achieve this goal, a co-operation component is added to the cost functional (performance index) of each agent. To this end, define w_(t) ^(i) as an information vector transmitted to agent A_(i) by other agents in its neighborhood G_(i), which consists of the states of these neighbors.

The external input to agent A_(i) in formation control task in a multi-agent system consists of state information of other agents in its neighborhood Gi, such that:

$\begin{matrix} {{w^{i}\overset{\bigtriangleup}{=}{{{{col}\left( x^{j} \right)}{\forall j}} = 1}},\ldots \mspace{11mu},M^{i},{j \in G^{i}},{j \neq i},} & (48) \end{matrix}$

where M^(i)is the number of agents in the neighborhood of A_(i). This external input in the form of the information vector w^(i) is driven by the dynamics of the neighboring systems, as below:

$\begin{matrix} {{w^{i}\overset{\Delta}{=}{{{{col}\left( x^{j} \right)}{\forall j}} = 1}},\ldots \mspace{14mu},M^{i},{j \in G^{i}},{j \neq i},} & (49) \end{matrix}$

where g^(i) is a nonlinear map composed of nonlinear dynamics of neighboring agents and their local inputs

$\varphi_{t}^{i}\overset{\Delta}{=}{{{col}\left( u_{t}^{j} \right)}.}$

We assume that the information vector is constrained to the following set:

$\begin{matrix} {{w_{t}^{i} \in W^{i} \Subset {\mathbb{R}}^{p^{i}}},{W^{i}\overset{\Delta}{=}{\left\{ {{w^{i}\text{∷}w_{\min}^{i}} \leq w^{i} \leq w_{\max}^{i} > 0} \right\}.}}} & (50) \end{matrix}$

Moreover, assume that we have an updatable approximation for w^(i), which produces the approximation {tilde over (w)}^(i), such that:

{tilde over (w)} _(t) ^(i) =w _(t) u+ξ_(w) _(t) ^(i), ξ _(w) ^(i)≦|ξ_(w) _(t) ^(i)≦ξ _(w) ^(i)|.   (51)

We assume that we do not have exact knowledge of the evolution of the information over the horizon i.e. w_(t,t+N) _(p) ^(i), and that we can only have an approximation of it {tilde over (w)}_(t,t+N) _(p) ^(i). Also, let us assume that the agent A_(i) has nominal model {tilde over (g)} (·) of the true information vector (49) given by:

{tilde over (w)} _(t+1) ^(o) ={tilde over (g)} ^(i l () {tilde over (w)} _(t) ^(i)),   (52)

such that there is a bounded information vector transition uncertainty due to information vector model mismatch:

{tilde over (g)}(w _(t) ^(i))=g(w _(t) ^(i), φ_(t) ^(i))+e _(w) _(t) ^(i) , e _(w) ^(i) ≦|e _(w) _(t) ^(i) |≦ē _(w) ^(i)   (53)

Now, let the distributed cost function of each agent be given as:

$\begin{matrix} {{{J_{t}^{i}\left( {{\overset{\sim}{x}}^{i},u^{i},{\overset{\sim}{w}}^{i},d^{h^{i}},{d^{q^{i}}N_{c}^{i}},N_{p}^{i}} \right)} = {{\sum\limits_{l = t}^{t + N_{c}^{i} - 1}\; \left\lbrack {{h^{i}\left( {{\overset{\sim}{x}}_{l}^{i},u_{l}^{i},d^{h^{i}}} \right)} + {q^{i}\left( {{\overset{\sim}{x}}_{l}^{i},{\overset{\sim}{w}}_{l}^{i},d^{q^{i}}} \right)}} \right\rbrack} + {\sum\limits_{l = t}^{t + N_{p - 1}^{i}}\left\lbrack {{h^{i}\left( {{\overset{\sim}{x}}_{l}^{i},{k_{f}^{i}\left( {\overset{\sim}{x}}_{l}^{i} \right)},d^{h^{i}}} \right)} + {q^{i}\left( {{\overset{\sim}{x}}_{l}^{i},{\overset{\sim}{w}}_{l}^{i},d^{q^{i}}} \right)}} \right\rbrack} + {h_{f}^{i}\left( {{\overset{\sim}{x}}_{t + N_{p}^{i}}^{i},d^{h^{i}}} \right)}}},} & (54) \end{matrix}$

where N_(p) ^(o)and N_(c) ^(i) are prediction and control horizons, respectively according to the NMPC notation. The distributed cost function (54) consists of: (i) Local transition costh^(i), which is the cost to reach a local target state, which is embedded in the local alignment vector d^(t) ^(i) ; (ii) cooperative cost q^(i), which is the cost for agent A^(i) to converge to an aligned state with its neighbors A^(j) ∈ G^(i), where the cooperation goal is embedded in the cooperative alignment vector d^(q) ^(i) ; and (iii) terminal cost h_(f) ^(i) is the cost of distance between the local state at t+N_(p) ^(i) and the local target state. Local control sequence u_(t,t+N) _(p) _(i) ^(i) consists of u_(t,t+N) _(C) _(i) ^(i l and u) _(t,t+N) _(c) _(i) _(+−N) _(p−1) _(i) ^(i) The latter part is generated by a local terminal control law u^(i)=k_(f) ^(i)({tilde over (x)}_(l) ^(i)), while the former is finite horizon optimal control u_(t,t+N) _(p) _(i) ^(i), which is the solution of the optimization problem (4). Now, in spite of the agents being dynamically decoupled, states of other agents in the multi-agent system affect the control of agent A^(i) by virtue of the information vector {tilde over (w)}^(i) being part of its NMPC cost function (54). Therefore, even though the agents are decoupled in the open loop, their dynamics is coupled in close loop due to cooperation cost component in distributed cost (54). Plots 2500 of FIG. 25 illustrate normalized cost of each vehicle in a 3 vehicle fleet. The spikes in cost occur when collision avoidance is active. The exemplary minimum separation of 5 units is not violated. Thus, the closed loop form of the open loop agent dynamics (45) is:

x _(t+1) ^(i) =f ^(i)(x _(t) ¹ , u _(t) ^(i) , w _(t) ^(i)), ∀t≧0, i=1, . . . , N.   (55)

We assume that our model of agent dynamics is not perfect, such that the nominal model used for control synthesis is:

{tilde over (x)} _(t+1) ^(i) =f ¹({tilde over (x)} _(t) ^(i) , u _(t) ^(i) , w _(t) ^(i)),   (56)

where, the actual state of the system is x_(t) ^(i), while the state predicted by model (49) is {tilde over (x)}_(t) ^(i). This system model mismatch leads to agent transition uncertainty such that:

{tilde over (f)} ^(i)(x _(t) , u _(t) , w _(t))=f ^(i)(x _(t) , u _(t) , w _(t))+e _(x) _(t) ^(i) , e _(x) ^(i) ≦|e _(x) _(t) ^(i) |≦e _(x) ¹.    (57)

Now, due to uncertainty, the constraint sets (5.2) and (5.6) for x^(i) and w^(i)will be ‘larger’ than constraint sets for {tilde over (x)}^(i), and {tilde over (w)}^(i), such that:

{tilde over (x)} _(t) ∈ {tilde over (X)} _(t) ^(i)(ē _(x) ^(i),{tilde over (ξ)}_(x) ^(i) , e _(w) ^(i), ξ _(w) ^(i)) ⊂ X ^(i) , u _(t) ∈ U, {tilde over (w)} _(t) ^(i) ∈ {tilde over (W)} _(t) ^(i)(ē _(w) ^(i), ξ _(w) ^(i)) ⊂ W ^(i)   (58)

Distributed finite horizon optimal control problem (OCP (4)): At every instant t≧0, given prediction and control horizonsN_(p) ^(i), N_(c) ^(i) ∈

_(≧0) terminal control k_(f) ^(i)({tilde over (x)}^(i)): R^(n)→R^(m), state estimate {tilde over (x)}_(t) ^(i) and information vector approximation {tilde over (w)}_(t+N) _(p) _(i) ^(i), find the optimal control sequence u_(t,t+N) _(C−1) ⁰ ^(i) that minimizes the finite horizon cost 5.26:

$\begin{matrix} {{u_{t,{t + N_{C - 1}}}^{0^{i}} = {\underset{u \in U}{\arg \; \min}{J_{t}\left( {\overset{\sim}{x},{\overset{\sim}{w}}_{t,{t + N_{p}}},u_{t,{t + N_{p}}},N_{c},N_{p}} \right)}}},} & (59) \end{matrix}$

subject to (I.) nominal state dynamics (56), (II) nominal information vector dynamics (52), (III) tightened constraint sets (58), (IV) Terminal state {tilde over (x)}_(t+N) _(p) ^(i) is constrained to an invariant terminal set X_(f) ^(i) ∈ {tilde over (X)}_(t+N) _(c) ^(i), i.e.:

{tilde over (x)} _(t+l) ^(i) ∈ X _(f) ^(i) , ∀l=N _(C) ^(i) , . . . , N _(p) ^(i)   (60)

The loop is closed by implementing only the first element of u_(t,t+N) _(c) ^(i) ⁻¹ ⁰ ^(i) at each instant such that the NLMPC control law becomes:

Θ_(t) ^(i)({tilde over (x)} ^(i) , {tilde over (w)} ^(i))=u _(t) ⁰ ^(i) ({tilde over (x)} _(t) ^(i) , {tilde over (w)} _(t) ^(i) , N _(p) ^(i) , N _(C) ^(i)),   (61)

and the closed loop dynamics becomes:

x _(t+1) ^(i) =f(x _(t) ^(i), Θ_(t) ^(i)({tilde over (x)} ^(i) , {tilde over (w)} ^(i)), w _(t) ^(i))=f _(C) ^(i)(x _(t) ^(i) , w _(t) ^(i)),   (62)

with local closed loop nonlinear map f_(C) ^(i)(x, w). This process is repeated every sampling instant, as illustrated in state and control plots 200 of FIG. 2. To summarize, at time t, each agent A^(i) (i=1, . . . , N) estimates its local state {tilde over (x)}_(t) ^(i) and receives an approximation of the information vector from its neighbors. Then, cost (54) is minimized over the finite horizon using the control adjustments and pre-computed terminal control law subject to constraints (58) and (60). Only the first element of this optimized control sequence is implemented. Then the cycle is repeated at the next sampling instant. The pictorial diagram 2600 indicates a network topology of a weakly connected team with one way communication between agents A⁴ and A⁵. Pictorial graph 2700 illustrates a fleet of 5 robots connected in a weakly connected network in a V-formation. Note the successful collision avoidance. Plots 2800 of FIG. 28 illustrate states of agents connected in the weakly connected network. Plot 2900 of FIG. 29 indicates a small gain condition for the first agent in the weakly connected team (design function parameter k1=5000) for a team of agents with dynamics controlled under eqn. (62). Regarding stability of individual agents with collision avoidance, for an agent on collision course, the optimal trajectory {acute over (x)}_(t,t+N) _(p) _(i) ^(i) ⁰ for cost (54) is modified as cost:

{tilde over (J)} _(t) ^(i) =J _(t) ^(i)(1+φ_(t) ^(i)).   (63)

A collision course 2100 as illustrated in FIG. 21 is defined as an agent A^(i) to be on collision course with at least one other agent, i.e.:

$\begin{matrix} {{{\sum\limits_{j\; ɛ\; G^{i}}1_{{{{Rimin} - {dijk}} > 0},{\forall{t \leq k \leq {t + N_{p}^{i}}}}}} > 0},{\forall{j \neq i}}} & (64) \end{matrix}$

where R_(min) safety zone of an agent and dijk is thhe Euclidan distance between two agents, the summation representing the total number of agents in collision course with agent A^(i). The repelling potential is formulated as:

$\begin{matrix} {\varphi_{t}^{i} = {\sum\limits_{j\; ɛ\; G^{i}}{\frac{\overset{\_}{\lambda}1_{{{{Rimin} - {dijk}} > 0},{\forall{t \leq k \leq {t + N_{p}^{i}}}}}}{\sum\limits_{k = t}^{t + N_{p}^{i}}\; {{\lambda \left( d_{k}^{ij} \right)}d_{k}^{ij}}}.}}} & (65) \end{matrix}$

Successful collision avoidance occurs if weighted average distance between the agents on collision course is increased during the next time instant i.e.:

$\begin{matrix} {{\sum\limits_{k = t}^{t + N_{p}^{i}}\; {{\lambda \left( d_{k}^{ij} \right)}d_{k}^{ij}}} < {\sum\limits_{k = {t + 1}}^{t + N_{p}^{i} + 1}\; {{\lambda \left( d_{k}^{ij} \right)}{d_{k}^{ij}.}}}} & (66) \end{matrix}$

For an agent on collision course the optimal trajectory with modified cost will avoid the collision while maintaining input-to-state practical stability if the repulsive spatial filter weights are computed at each sampling instant t as follows:

$\begin{matrix} {{\frac{\lambda_{\max,t}^{i}}{\lambda_{\min,t}^{i}} < \frac{{\underset{\_}{r}}^{i}\left( {x_{t}} \right)}{\begin{matrix} \left( {{\left( {N_{p}^{i} - 1} \right)\left( {L_{hx}^{i} + L_{qx}^{i}} \right)} + L_{hf}} \right) \\ \left( {{N_{p}^{i}R_{\min}} + {{N_{p}^{i}\left( {N_{p}^{i} - 1} \right)}v_{\max}}} \right) \end{matrix}}}\overset{\bigtriangleup}{=}{{\overset{\_}{a}}_{t}.}} & (67) \end{matrix}$

Successful collision avoidance is illustrated in FIG. 22 where in map 2202 the agents were on a collision course, but the collision avoidance component of the present method pushed them away from collision as indicated in map 2204.

With respect to neural network based trajectory compression, for cooperation, agents transmit their planned state trajectories as mentioned supra. These communication packets are received by vehicles within the neighborhood of transmitting agents. Neighborhood may be defined based on communication range, number of channels on receiving agents etc. With reference to neural network compression 2000 of FIG. 20, a trajectory compressed at Agent A^(j) is transmitted to Agent A^(i), where it is received after delay Δ_(ij) and recovered using the neural network. An exemplary communications packet is shown in Table 7.

TABLE 7 Anatomy of a Typical Communication Packet Data Register Data 1 Agent identity, i 2 Time stamp, T_(s) ^(i) 3 Sampling time, T^(i) 4 to 3 + q^(i) Neural network, N^(i) 4 + q^(i) onwards Error correcting codes Optional (leader) Cooperation Goals To reduce packet size, this trajectory containing n^(i)×N_(p) ^(i) floating points is compressed by approximating it with neural network N^(i) of q^(i) weights and biases, with compression factor C_(w) ^(i) of

$\begin{matrix} {C_{w}^{i} = {1 - {\frac{q^{i} + {{overhead}\mspace{14mu} {size}}}{n^{i} \times N_{p}^{i}}.}}} & (68) \end{matrix}$

Tail recovery of a useful part of the trajectory at reception time t is accomplished by tail prediction as follows:

{tilde over (w)} _(t+N) _(p) _(i) _(−Δ) _(ij) ₊₁ ^(i) ={tilde over (g)} ^(i)({tilde over (w)} _(t+N) _(p) _(i) _(−Δ) _(ij) ^(i)), . . . {tilde over (w)} _(t+N) _(p) _(i) ^(i) ={tilde over (g)} ^(i)({tilde over (w)} _(t+N) _(p) _(i) ⁻¹ ^(i))   (69)

Preferred neural network for this computation is a two layer NN although other NN topologies are contemplated by the present method.

The Distributed NMPC Algorithm for Formation Control procedure is summarized in Table 8.

TABLE 8 Algorithm 7 Distributed NMPC Algorithm for Formation Control 1: procedure OFFLINE CONVEX and MIN-MAX OPTIMIZATION. 2: ${{Input}\mspace{14mu} A^{1}1},\left. A^{i}\leftarrow{\overset{\sim}{x}}_{t}^{i} \right.,d^{h^{i}},d^{q^{i}},{{g^{i} \vartriangleleft i} = {1\overset{\Delta}{=}{Leader}}},{t = 0},$ 3: Tighten constraints with Algorithm 8 4: Compute Q_(f) ^(i), K_(f) ^(i) using Algorithms 3 and 4 5: Compute Output deasibility set X_(MPC) ^(i) and controlability sets C₁(X_(f) ^(i)) using Algorithms 5 and 6 6: end procedure 7: procedure DISTRIBUTED ONLINE RH OPTIMIZATION 8: Design Spatially filtered potential (67) 9: Solve Problem ECP (4) at A^(i) for Q_(t,t+N) _(C−1) _(i) ^(i0) 10: Train NN Train Neural network for {tilde over (x)}_(t,t+N) _(p) ^(i) ⁰ 11: Implement first element/block of Q_(t,t+N) _(C−1) _(i) ^(i0) 12: Transmit/Receive data packets 13: Estimate time delay Δ_(ij) 14: Reconstruct {tilde over (w)}_(t,t+N) _(p) _(i) ^(i) with received NN and estimate tail if received trajectory (69). Increment time by one sample

 t^(i) = t^(i) + T^(i) Multi-agent prediction error bounds are analogous to the single agent prediction error bounds developed supra. With actual constraints X^(i) and W^(i), the tightened constraints are given by:

$\begin{matrix} {{{\overset{\sim}{X}}_{t + l}^{i}\overset{\bigtriangleup}{=}{\left. X^{i} \right.\sim{\beta^{n^{i}}\left( {\overset{\_}{\rho}}_{x_{t + l}}^{i} \right)}}},} & (70) \\ {and} & \; \\ {{{\overset{\sim}{W}}_{t + l}^{i}\overset{\bigtriangleup}{=}{\left. W^{i} \right.\sim{\beta^{p^{i}}\left( {\overset{\_}{\rho}}_{w_{t + l}}^{i} \right)}}},} & (71) \end{matrix}$

The prediction error bound {tilde over (ρ)}^(i) _(x)is defined as:

$\begin{matrix} {{{\overset{\_}{\rho}}_{x_{t + l}}^{i}\overset{\bigtriangleup}{=}{{L_{fx}^{i^{l}}{\overset{\_}{\xi}}_{x}^{i}} + {{\overset{\_}{e}}_{x}^{i}\frac{L_{fx}^{i^{l}} - 1}{L_{fx}^{i} - 1}}}},} & (72) \\ {and} & \; \\ {{\overset{\_}{\rho}}_{w_{t + l}}^{i}\overset{\bigtriangleup}{=}{\sum\limits_{j\; ɛ\; G^{i}}{{{w_{t + 1}^{i} - {\overset{\sim}{w}}_{t + 1}^{i}}}_{j}.}}} & (73) \end{matrix}$

The constraint tightening procedure for multi-agents distributed processing is summarized in Table 9.

TABLE 9 Algorithm 8 Agent Constraint Tightening 1: Given i nominal models {tilde over (f)}^(l)({tilde over (x)}^(i),u^(i){tilde over (w)}^(i)), {tilde over (g)}^(i) ({tilde over (w)}^(i)), uncertainty bounds ξ _(w) ^(i), ξ _(x) ^(i), ē_(x) ^(i), ē_(w) ^(i), and horizons N_(C) ^(i), N_(P) ^(i). 2: procedure CONSTRAINT TIGHTENING 3: Calculate Lipschitz constants of nonlinear maps {tilde over (f)}^(l)({tilde over (x)}^(i),u^(i){tilde over (w)}^(i)) and {tilde over (g)}^(i) ({tilde over (w)}^(i)). 4: Calculate the prediction error bounds in (72) and (73).. 5: Tighten the constraints by Pontryagin difference as given in (70)-(71). 6: end procedure

It is to be understood that the present invention is not limited to the embodiments described above, but encompasses any and all embodiments within the scope of the following claims. 

We claim:
 1. A computer-implemented control method for multi-vehicle systems, comprising the steps of: optimizing trajectories of a plurality of autonomous vehicles (mobile robots); predicting states of the vehicles; determining tightened constraints on the vehicle states, the tightened constraints being characterized by the relations: ${{\overset{\sim}{X}}_{t + l}^{i}\overset{\bigtriangleup}{=}{\left. X^{i} \right.\sim{\beta^{n^{i}}\left( {\overset{\_}{\rho}}_{x_{t + l}}^{i} \right)}}},{and}$ ${{\overset{\sim}{W}}_{t + l}^{i}\overset{\bigtriangleup}{=}{\left. W^{i} \right.\sim{\beta^{p^{i}}\left( {\overset{\_}{\rho}}_{w_{t + l}}^{i} \right)}}};$ and estimating new states of the vehicles based on a result of the state-predicting step and the tightened constraints determination step; wherein a prediction error bound ρ ^(i) _(x) is defined as: ${{\overset{\_}{\rho}}_{x_{t + 1}}^{i}\overset{\Delta}{=}{{L_{fx}^{i^{l}}{\overset{\_}{\xi}}_{x}^{i}} + {{\overset{\_}{e}}_{x}^{i}\frac{L_{fx}^{i^{l}} - 1}{L_{fx}^{i}}}}},{and}$ ${\overset{\_}{\rho}}_{w_{t + 1}}^{i}\overset{\Delta}{=}{\sum\limits_{j \in G^{i}}{{{w_{t + l}^{i} - {\overset{\sim}{w}}_{t + l}^{i}}}_{j}.}}$
 2. The computer-implemented control method for multi-vehicle systems according to claim 1, further comprising the steps of: inputting a nominal model {tilde over (f)}({tilde over (x)}, u, 0), nominal constraints, a receding horizon (RH) cost, and error bounds; determining optimized terminal set X_(f) and terminal control k_(f); warm starting a terminal constraint region; determining a one-step controllability set C₁(X_(f)) to ensure recursive feasibility; determining a robust output feasibility set X_(MPC); measuring outputs {tilde over (y)}_(t+1) and disturbance {tilde over (w)}_(t+1); estimating state {tilde over (x)}_(t+l) and disturbance {tilde over (w)}_(t+l); solving finite horizon OCP at t+l for control u_(t+1,t+l,t+N) _(c) ⁰; and implementing a first element of optimized control u_(t) ^(0.)
 3. The computer-implemented control method for multi-vehicle systems according to claim 2, further comprising the steps of: calculating Lipschitz constants of nonlinear maps {tilde over (f)}({tilde over (x)},u, {tilde over (w)}) and {tilde over (g)}({tilde over (w)}); and using the Lipschitz constants in the tightening constraints step of claim
 1. 4. The computer-implemented control method for multi-vehicle systems according to claim 3, further comprising the steps of: selecting S ∈

^(n×n), such that −q({tilde over (x)},{tilde over (w)})+ψ({tilde over (w)})≦{tilde over (x)}_(c) ^(i){tilde over (S)}{tilde over (x)}, given the nominal model {tilde over (f)}({tilde over (x)},u, 0)), and cost weights Q, R and S; obtaining initial guess values of Q_(f) as Q_(f) ^(∞) and K as K^(∞); solving a convex optimal control problem (OCP (A)) using parameterized state and control constraints characterized by the relations: $\left\lbrack {\begin{matrix} {{x_{t} \in X \Subset R^{n}},} & {X,\left\{ {x:{x_{\min} \leq x \leq x_{\max} > 0}} \right\}} \\ {{y_{t} \in Y \Subset R^{q}},} & {Y,\left\{ {y:{y_{\min} \leq y \leq y_{\max} > 0}} \right\}} \\ {{u_{t} \in U \Subset R^{m}},} & {U,\left\{ {u:{u_{\min} \leq u \leq u_{\max} > 0}} \right\}} \\ {{w_{t} \in W \Subset R^{p}},} & {W,\left\{ {w:{w_{\min} \leq w \leq w_{\max} > 0}} \right\}} \end{matrix},} \right\rbrack$ and subject to formulaic computations characterized by the relations: ${W_{1} = {W_{1}^{T} > 0}},{a > 0},{\begin{bmatrix} W_{1} & \left( {{A_{v}W_{1}} + {B_{v}W_{2}}} \right)^{T} & {W_{1}\left( {Q - \overset{\sim}{S}} \right)}^{1/2} & {W_{2}^{T}R^{1/2}} \\ * & W_{1} & 0 & 0 \\ * & * & I & 0 \\ * & * & * & I \end{bmatrix} \geq 0},$ for v=1, . . . , v, ${\begin{bmatrix} {1/a} & \left( {{{\overset{\_}{c}}_{v}W_{1}} + {{\overset{\_}{d}}_{v}W_{2}}} \right)^{T} \\ * & W_{1} \end{bmatrix} \geq 0},$ determining whether X_(f) ⊂ {tilde over (X)}_(t+N) _(p) ; solving (if X_(f) is not a subset of {tilde over (X)}_(t+N) _(p) ) the convex OCP (A) subject to an additional condition characterized by the relation: ${\begin{bmatrix} {- \left( {Q - \left( {\overset{\_}{S} + {\hat{a}I_{n}}} \right)} \right)} & W_{2}^{T} \\ * & R^{- 1} \end{bmatrix} \geq 0};$ and accepting optimal values of Q_(f), K and a.
 5. The computer-implemented control method for multi-vehicle systems according to claim 4, wherein the warm starting step further comprises the steps of: solving Riccati equations for vertex values of Q_(f) _(v) ^(∞), the Riccati equations being a formula characterized by the relation: Q _(f) _(v) ^(∞)=(Q−{tilde over (S)})+A _(v) ^(T)(Q _(f) _(v) ^(∞) +Q _(f) _(v) ^(∞) B _(v)(R+B _(v) ^(T) Q _(f) _(v) ^(∞) B _(v))⁻¹ B _(v) ^(T) Q _(f) _(v) ^(∞))A _(v), where Q_(f) _(v) ^(∞) is a solution to the discrete-time algebraic Riccati equations (DARE) at each vertex point; solving convex OCP (2) to obtain Q_(f) ^(∞); and calculating K^(∞) by solving a formula for Q_(f) ^(∞) at A₀ and B₀, the formula being characterized by the relation: K _(v) ^(∞)=(R+B _(v) ^(T) Q _(f) _(v) ^(∞) B _(v))⁻¹ B _(v) ^(T) Q _(f) _(v) ^(∞) A _(v).
 6. The computer-implemented control method for multi-vehicle systems according to claim 5, wherein the one-step controllability set determining step further comprises the steps of: dividing a boundary of terminal set∂(X_(f)) into Ñ steps; solving OCP (3) to find points {tilde over (x)}_(c) ^(i) ∈ ∂ (C₁(X_(f))) for i=1, . . . , N; and calculating a minimum size of C₁(X_(f)) as d=min(|{tilde over (x)}_(c) ₁ ¹−{tilde over (x)}_(f) ¹|, . . . , |{tilde over (x)}_(c) ₁ ^(N) −{tilde over (x)}_(f) ^(Ñ)|) for {tilde over (x)}_(f) ^(i) ∈ ∂(X_(f)) and i=1, . . . , N.
 7. The computer-implemented control method for multi-vehicle systems according to claim 6, further comprising the steps of: determining C₁(X_(f)) by using the steps of claim 6, given as {tilde over (x)}_(C) ₁ ^(i) ∈ ∂ (C₁(X_(f))) for i=1, . . . , N; recursively estimating X_(MPC) for l=2, . . . , N_(C) by: solving OCP (3) with target set C₁(X_(f)) to obtain C₂(X_(f))=C₁(C₁(X_(f))), when l=2; solving OCP (3) with target set C_(l−1)(X_(f)) to obtain C_(l)(X_(f))=C₁(C_(l−1)(X_(f))), when l≠2; and determining X_(MPC) according to a formula characterized by the relation: X _(MPC) =U _(l=2) ^(l=N) ^(c) C ₁(C _(l−1)(X _(f))) ∪ C ₁(X _(f)) ∪ X _(f).
 8. The computer-implemented control method for multi-vehicle systems according to claim 7, wherein the vehicles are communicating in a network, the method further comprising the steps of: ${{inputting}\mspace{20mu} A^{1}1},\left. A^{i}\leftarrow{\overset{\sim}{x}}_{t}^{i} \right.,d^{h^{i}},d^{q^{i}},{{g^{i} \vartriangleleft i} = {1\overset{\Delta}{=}\mspace{11mu} {Leader}}},{{t = 0};}$ computing Q_(f) ^(i), K_(f) ^(i); computing output feasibility set X_(MPC) ^(i) and controllability sets C₁(X_(f) ^(i)); designing a spatially filtered potential according to a formula characterized by the relation: ${{\frac{\lambda_{\max,t}^{i}}{\lambda_{\min,t}^{i}} < \frac{{\underset{\_}{r}}^{i}\left( {x_{t}} \right)}{\left( {{\left( {N_{p}^{i} - 1} \right)\left( {L_{hx}^{i} + L_{qx}^{i}} \right)} + L_{hf}} \right)\left( {{N_{p}^{i}R_{\min}} + {{N_{p}^{i}\left( {N_{p}^{i} - 1} \right)}v_{\max}}} \right)}}\overset{\Delta}{=}{\overset{\_}{a}}_{t}};$ solving OCP (4) at A^(i)for Q_(t,t+N) _(C−1) _(i) ^(i) ⁰ ; training a neural network (NN) for {tilde over (x)}_(t,t+N) _(p) ^(i) ⁰ ; implementing a first element block of u_(t,t+N) _(C−1) _(i) ^(i) ⁰ ; transmitting and receiving data packets; estimating a time delay Δ_(ij); reconstructing {tilde over (w)}_(t,t+N) _(p) _(i) ^(i) with received NN; and estimating a tail of received trajectory according to a formula characterized by the relation: {tilde over (w)} _(t+N) _(p) _(i) _(−Δ) _(ij) ₊₁ ^(i) ={tilde over (g)} ^(i)({tilde over (w)} _(t+N) _(p) _(i) _(−Δ) _(ij) ^(i)), . . . {tilde over (w)} _(t+N) _(p) _(i) ^(i) ={tilde over (g)} ^(i)({tilde over (w)} _(t+N) _(p) _(i) ⁻¹ ^(i)).
 9. A control system for multi-vehicle systems having a plurality of autonomous vehicles (mobile robots), the control system comprising in each of the autonomous vehicles: an optimizer outputting control signals to the vehicle; a state predictor connected to the optimizer; a state estimator connected to the state predictor, the state estimator accepting information about the vehicle's state as input and outputting its estimate to the state predictor; and means for determining tightened constraints on the vehicle states, the tightened constraints being characterized by the relations: ${{\overset{\sim}{X}}_{t + 1}^{i}\overset{\Delta}{=}{\left. X^{i} \right.\sim{\beta^{n^{i}}\left( {\overset{\_}{\rho}}_{x_{t + l}}^{i} \right)}}},{and}$ ${{\overset{\sim}{W}}_{t + 1}^{i}\overset{\Delta}{=}{\left. W^{i} \right.\sim{\beta^{p^{i}}\left( {\overset{\_}{\rho}}_{w_{t + l}}^{i} \right)}}};$ wherein a prediction error bound ρ ^(i) _(x)is defined as ${{\overset{\_}{\rho}}_{x_{t + 1}}^{i}\overset{\Delta}{=}{{L_{fx}^{i^{l}}{\overset{\_}{\xi}}_{x}^{i}} + {{\overset{\_}{e}}_{x}^{i}\frac{L_{fx}^{i^{l}} - 1}{L_{fx}^{i}}}}},{and}$ ${\overset{\_}{\rho}}_{w_{t + 1}}^{i}\overset{\Delta}{=}{\sum\limits_{j \in G^{i}}{{{w_{t + l}^{i} - {\overset{\sim}{w}}_{t + l}^{i}}}_{j}.}}$
 10. The control system for multi-vehicle systems according to claim 9, further comprising: means for inputting a nominal model {tilde over (f)}({tilde over (x)},u, 0), nominal constraints, a receding horizon (RH) cost, and error bounds; means for determining optimized terminal set X_(f)and terminal control k_(f); means for warm starting a terminal constraint region; means for determining a one-step controllability set C₁(X_(f)) to ensure recursive feasibility; means for determining a robust output feasibility set X_(MPC); means for measuring outputs {tilde over (y)}_(t+1) and disturbance {tilde over (w)}_(t+1); means for estimating state {tilde over (x)}_(t+1) and disturbance {tilde over (w)}_(t+l); means for solving finite horizon OCP at t+1 for control u_(t+1,t+l,t+N) _(c) ⁰; and means for implementing a first element of optimized control u_(t) ⁰.
 11. The control system for multi-vehicle systems according to claim 10, further comprising: means for calculating Lipschitz constants of nonlinear maps {tilde over (f)}({tilde over (x)},u,{tilde over (w)}) and {tilde over (g)}({tilde over (w)}); and means for using the Lipschitz constants in the tightening constraints step of claim
 9. 12. The control system for multi-vehicle systems according to claim 11, further comprising: means for selecting {tilde over (S)} ∈

^(n×n), such that −q({tilde over (x)},{tilde over (w)})+ψ({tilde over (w)})≦{tilde over (x)}_(c) ^(i){tilde over (S)}{tilde over (x)}, given the nominal model {tilde over (f)}({tilde over (x)}, u, 0)), and cost weights Q, R and S; means for obtaining initial guess values of Q_(f) as Q_(f) ^(∞) and K as K^(∞); means for solving a convex optimal control problem (OCP (A)) using parameterized state and control constraints characterized by the relations: $\left\lbrack {\begin{matrix} {{x_{t} \in X \Subset R^{n}},} & {X,\left\{ {x:{x_{\min} \leq x \leq x_{\max} > 0}} \right\}} \\ {{y_{t} \in Y \Subset R^{q}},} & {Y,\left\{ {y:{y_{\min} \leq y \leq y_{\max} > 0}} \right\}} \\ {{u_{t} \in U \Subset R^{m}},} & {U,\left\{ {u:{u_{\min} \leq u \leq u_{\max} > 0}} \right\}} \\ {{w_{t} \in W \Subset R^{p}},} & {W,\left\{ {w:{w_{\min} \leq w \leq w_{\max} > 0}} \right\}} \end{matrix},} \right\rbrack$ and subject to formulaic computations characterized by the relations: ${W_{1} = {W_{1}^{T} > 0}},{a > 0},{\begin{bmatrix} W_{1} & \left( {{A_{v}W_{1}} + {B_{v}W_{2}}} \right)^{T} & {W_{1}\left( {Q - \overset{\sim}{S}} \right)}^{1/2} & {W_{2}^{T}R^{1/2}} \\ * & W_{1} & 0 & 0 \\ * & * & I & 0 \\ * & * & * & I \end{bmatrix} \geq 0},$ for v=1, . . . , v, ${\begin{bmatrix} {1/a} & \left( {{{\overset{\_}{c}}_{v}W_{1}} + {{\overset{\_}{d}}_{v}W_{2}}} \right)^{T} \\ * & W_{1} \end{bmatrix} \geq 0},$ means for determining whether X_(f) ⊂ {tilde over (X)}_(t+N) _(p) ; means for solving (if X_(f) is not a subset of {tilde over (X)}_(t+N) _(p) ) the convex OCP (A) subject to an additional condition characterized by the relation: ${\begin{bmatrix} {- \left( {Q - \left( {\overset{\_}{S} + {\hat{a}I_{n}}} \right)} \right)} & W_{2}^{T} \\ * & R^{- 1} \end{bmatrix} \geq 0};$ and means for accepting optimal values of Q_(f), K and a.
 13. The control system for multi-vehicle systems according to claim 12, further comprising: means for solving Riccati equations for vertex values of Q_(f) _(v) ^(∞), the Riccati equations being a formula characterized by the relation: Q _(f) _(v) ^(∞)=(Q−{tilde over (S)})+A _(v) ^(T)(Q _(f) _(v) ^(∞) +Q _(f) _(v) ^(∞) B _(v)(R+B _(v) ^(T) Q _(f) _(v) ^(∞) B _(v))⁻¹ B _(v) ^(T) Q _(f) _(v) ^(∞))A _(v), where Q_(f) _(v) ^(∞) is a solution to the discrete-time algebraic Riccati equations (DARE) at each vertex point; means for solving convex OCP (2) to obtain Q_(f) ^(∞); and means for calculating K^(∞) by solving a formula for Q_(f) ^(∞) at A₀ and B₀, the formula being characterized by the relation: K _(v) ^(∞)=(R+B _(v) ^(T) Q _(f) _(v) ^(∞) B _(v))⁻¹ B _(v) ^(T) Q _(f) _(v) ^(∞) A _(v).
 14. The control system for multi-vehicle systems according to claim 13, further comprising: means for solving OCP (3) to find points {tilde over (x)}_(c) ^(i) ∈ ∂ (C₁(X_(f))) for i=1, . . . , N; and means for calculating a minimum size of C₁(X_(f)) as d=min(|{tilde over (x)}_(c) ₁ ¹−{tilde over (x)}_(f) ¹|, . . . , |{tilde over (x)}_(c) ₁ ^(N) −{tilde over (x)}_(f) ^(N) |) for {tilde over (x)}_(f) ^(i) ∈ ∂(X_(f)) and i=1, . . . , N.
 15. The control system for multi-vehicle systems according to claim 14, further comprising: means for determining C₁(X_(f)) by using the steps of claim 6, given as {tilde over (x)}_(C) ₁ ^(i) ∈ ∂ (C₁(X_(f))) for i=1, . . . , N; means for recursively estimating X_(MPC) for l=2, . . . , N_(C) by: means for solving OCP (3) with target set C₁(X_(f)) to obtain C₂(X_(f))=C₁(C₁(X_(f))), when l=2; means for solving OCP (3) with target set C_(l−1)(X_(f)) to obtain C_(l)(X_(f))=C₁ (C_(l−1)(X_(f))) , when i≠2; and means for determining X_(MPC) according to a formula characterized by the relation: X _(MPC)=∪_(l=2) ^(l=N) ^(c) C ₁(C _(l−1)(X _(f))) ∪ C ₁(X _(f)) ∪ X_(f).
 16. The control system for multi-vehicle systems according to claim 15, where the vehicles are communicating in a network, further comprising: means for inputting ${A^{1}1},\left. A^{i}\leftarrow x_{t}^{i} \right.,d^{h^{i}},d^{q^{i}},{{g^{i} \vartriangleleft i} = {1\overset{\Delta}{=}{Leader}}},{{t = 0};}$ computing Q_(f) ^(i), K_(f) ^(i); means for computing output feasibility set X_(MPC) ^(i) and controllability sets C₁(X_(f) ^(i)); means for designing a spatially filtered potential according to a formula characterized by the relation: ${{\frac{\lambda_{\max,t}^{i}}{\lambda_{\min,t}^{i}} < \frac{{\underset{\_}{r}}^{i}\left( {x_{t}} \right)}{\left( {{\left( {N_{p}^{i} - 1} \right)\left( {L_{hx}^{i} + L_{qx}^{i}} \right)} + L_{hf}} \right)\left( {{N_{p}^{i}R_{\min}} + {{N_{p}^{i}\left( {N_{p}^{i} - 1} \right)}v_{\max}}} \right)}}\overset{\Delta}{=}{\overset{\_}{a}}_{t}};$ means for solving OCP (4) at A^(i) or Q_(t,t+N) _(C−1) ^(i) ^(i) ⁰ ; means for training a neural network (NN) for {tilde over (x)}_(t,t+N) _(p) ^(i) ⁰ ; means for implementing a first element block of u_(t,t+N) _(C−1) _(i) ^(i) ⁰ ; means for transmitting and receiving data packets; means for estimating a time delay Δ_(ij); means for reconstructing {tilde over (w)}_(t,t+N) _(p) _(i) ^(i) with received NN; and means for estimating a tail of received trajectory according to a formula characterized by the relation: {tilde over (w)} _(t+N) _(p) _(i) _(−Δ) _(ij) ₊₁ ^(i) ={tilde over (g)} ^(i) ({tilde over (w)} _(t+N) _(p) _(i) _(−Δ) _(ij) ^(i)), . . . {tilde over (w)} _(t+N) _(p) _(i) ^(i) ={tilde over (g)} ^(i) ({tilde over (w)} _(t+N) _(p) _(i) ⁻¹ ^(i)). 